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ABSTRACT 


An  iterative  least  square  estimation  algorithm  is  derived 
and  applied  to  the  problem  of  state  estimation  of  ballistic 
trajectories  with  angle  only  measurements.  A filter  initiation 
procedure  is  suggested.  The  application  of  trajectory  a priori 
knowledge  for  improving  the  estimate  is  discussed  and  solved  as 
a constrained  estimation  problem.  A Monte  Carlo  simulation  study 
was  conducted  to  evaluate  these  techniques.  It  was  found  that  the 
iterative  least  square  filter  achieves  the  Cramer-Rao  bound  and 
it  performs  better  than  the  extended  Kalman  filter  when  the 
sensor  is  on  a free-falling  platform.  When  the  sensor  is  on  a 
stationary  platform  however,  both  estimators  asymptotically 
achieve  the  Cramer-Rao  bound. 
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1. 


INTRODUCTION 


The  state  estimation  of  a ballistic  trajectory  with  angle 
only  measurements  is  a challenging  problem.  The  problem  becomes 
even  more  complex  when  the  sensor  is  placed  on  a free-falling 
platform.  It  is  difficult  to  initiate  a filter  for  such  systems. 
Furthermore,  the  extended  Kalman  filter  (EKF)  is  expected  to  perform 
poorly  since  the  EKF  performance  is  conditioned  on  being  able  to 
linearize  the  system  about  accurate  state  estimates. 

In  two  previous  reports,  [1]  and  [2],  the  Cramer-Rao 
lower  bound  on  the  covariance  of  the  trajectory  estimates  was 
presented  for  stationary  sensor  platforms  [1]  as  well  as  free- 
falling  sensor  platforms  [2]..  Since  this  bound  is  calculated  using 
the  information  matrix  (the  inverse  of  the  covariance  matrix)  and 
it  is  not  tied  with  any  specific  estimator,  the  results  can  be 
easily  calculated  and  they  are  not  restrained  by  the  filter  initi- 
ation problem.  Issues  remaining  to  be  addressed  include:  how  tight 
is  this  bound?  can  any  filter  achieve  this  bound? 

In  this  report,  we  present  an  iterative  least  square  (ILS) 
algorithm  for  estimating  the  state  of  nonlinear  deterministic  systems 
with  nonlinear  noisy  measurements.  It  is  known  that  the  exo- 
atmospheric  part  of  ballistic  trajectories  can  be  described  by 
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nonlinear  differential  equations  with  zero  process  noise.  This 
fact  enables  the  above  algorithm  to  be  applied  to  the  problem 
of  tracking  with  angle-only  measurements.  We  have  also  derived 
a procedure  using  a polynomial  fit  and  vehicle-sensor  dynamics 
for  calculating  filter  initial  conditions.  We  apply  this 
procedure  to  initialize  both  ILS  and  EKF  filters  and  evaluate 
their  performances  using  a Monte  Carlo  simulation. 

Since  the  angle-only  tracking  system  is  only  "weakly" 
observable*,  the  resulting  estimation  error  is  inevitably  large. 

It  is  suspected  that  trajectory  a priori  knowledge  (e.g., 
constraints  on  velocity, energy , angles,  etc.)  may  be  helpful  for 
improving  the  estimates.  This  constitutes  a constrained  estimation 
problem.  We  formulate  this  problem  and  present  a solution 
procedure. 

This  report  is  organized  as  follows.  In  the  next 
section,  we  state  the  trajectory  equation  of  motion.  In  the 
third  section,  we  derive  the  iterative  least  square  algorithm. 

Also,  presented  are  initiation  and  constrained  estimation 
procedures.  In  the  fourth  section  we  present  a numerical  example 
illustrating  the  ILS  and  EKF  performances  and  compare  them  with 
the  Cramer-Rao  bound. 


♦Since  range  is  not  measured. 


2.  TRAJECTORY  EQUATION  OF  MOTION 

Tracking  with  angle-only  measurements  is  primarily 
concerned  with  exo-atmospheric  trajectories.  Gravity  is 
therefore  the  most  significant  force  term  on  the  target.  For 
the  case  of  a free-falling  sensor  platform,  the  difference  of 
gravity  on  the  sensor  and  on  the  target  produces  curvature  in 
the  relative  sensor- target  trajectory  and  is  the  information  en- 
abling the  target  state  to  be  estimated.  For  these  reasons,  we 
consider  the  gravity  as  the  only  driving  force  on  the  trajectory. 
Furthermore,  we  use  a spherical  earth  model  to  simplify  the 
mathematics. 

Consider  a Cartesian  coordinate  with  origin  at  the 
center  of  the  earth.  The  trajectory  differential  equations  of 
motion  are 


x = -g 


0 i<V«2)3" 


_ y V 

y = "9°  {x2+y2+z2) 3^2 


(2.1) 


z = -g 


(x  +y  +z  ; 


where  gQ  is  the  gravity  at  sea  level  and  is  the  radius  of  the 
earth. 


*?> 


The  sensor  measures  azimuth  (A)  and  elevation  (E) 


angles  of  the  target  relative  to  the  sensor,  i.e., 
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x 

y 
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(2.2) 


where  the  states  used  in  (2.2)  are  the  difference  of  the  target 

state  x_  and  the  sensor  state  x , i.e.,  Eq.  (2.2)  is  evaluated  at 
X s 

- " -T  ~ -s  (2.3) 

When  the  sensor  is  stationary,  xg  is  a fixed  point  in  space. 

When  the  sensor  is  free-falling,  x is  described  by  the  same 
differential  equation  of  motion  Eq.  (2.1)  as  that  used  for  targets. 

The  sensor  measurement  noise  is  assumed  to  be  a white 
noise  sequence  with  covariance 


R 


cov (E, A) 
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cos  E . 


(2.4) 


where  a is  the  sensor  angle  measurement  standard  deviation. 
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Equations  (2.1)  - (2.4)  will  be  used  as  system  and 
measurement  equations  in  the  iterative  least  square  algorithm  to 
be  described  in  the  next  section.  Trajectory  equations  of  motion 
described  in  the  sensor  coordinates  will  be  found  useful  in  computing 
an  initial  guess  for  the  iterative  least  square  algorithm.  We 
therefore  state  them  below.  If  the  sensor  is  free-falling,  the 
target  differential  equations  of  motion  in  the  sensor  (R,A,E , ) 
coordinates  are 


••  *2*2  2 sift  E 

R = R(E  +A  cos  E) 
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fR  2 ( R+R  sin  E) 
s s 


-1 


T 
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(2.5c) 


where  R is  the  distance  between  target  and  sensor,  R^  is  the 

distance  between  target  and  the  earth  center,  and  R is  the  distance 

s 

between  sensor  and  the  earth  center.  The  R^,  is  related  to  R 

and  R by 
s 


Rtj,  - (R2  + Rg2  + 2RRS  sin  E) 1/2 


(2.6) 


Notice  that  the  gravity  appears  only  in  R and  E.  The  target- 
sensor geometry  is  illustrated  in  Fig.  2.1. 
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If  the  sensor  is  stationary,  the  gravity  term  on  range 
(gR)  and  elevation  (g£)  are  modified  as 


gR  = -gQ  —j  (R+Rssin  E) 

(2.7) 


«E  - -go  rzs  Vos  E 


If  the  sensor  is  stationary  and  the  target  is  relatively 
close  to  the  sensor,  a flat  earth  model  may  suffice.  The  gravi- 
tational terms  for  the  flat  earth  are 


gR  = -g0  sin  E 


cos  E 


(2.8) 
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3. 


ESTIMATION  ALGORITHM 


In  the  first  subsection,  we  describe  an  iterative 
least  square  algorithm  applicable  to  state  estimation  with 
deterministic  nonlinear  discrete  systems.  The  convergence  of  the 
iterative  least  square  algorithm  is  dependent  upon  a particular 
application  and  the  initial  guess  (initial  state)  for  the  itera- 
tive procedure.  In  the  second  subsection,  we  demonstrate  a method 
for  computing  an  initial  state  by  using  a batch  of  angle  measurements 
In  the  ballistic  trajectory  estimation  problem,  several 
variables  (e.g.,  re-entry  velocity,  re-entry  angle,  etc.) 
are  known  to  within  a certain  range  of  values.  Incorporating  this 
a priori  information  with  measurements  to  obtain  a "combined" 
estimate  constitutes  a constrained  estimation  problem.  In  the  third 
subsection,  we  present  algorithms  for  calculating  the  constrained 
estimate.  In  the  fourth  subsection,  we  combine  these  analyses  to 
present  two  algorithms  for  ballistic  trajectory  tracking  applications 

3.1.  An  Iterative  Least  Square  Algorithm 

Consider  the  following  nonlinear  discrete  system  and 
measurement  equations: 

xn+1  = fUn),  n*l,...  (3.1) 

*n+l  = &<W  + ~n+l  (3'2) 

where  x is  the  state  vector,  y is  the  measurement  vector,  ^ is 
the  noise  corrupted  measurement  vector,  v is  the  measurement 
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noise  vector  and  n is  the  discrete  time  index.  We  assume  that  vn 

is  a time-wise  uncorrelated  random  sequence  which  is  Gaussian 

with  zero  mean  and  covariance  R . Notice  that  we  do  not  include 

n 

(1)  process  noise  and  (2)  unknown  time-varying  parameters*  in 
Eq.  (3.1).  These  assumptions  are  valid  for  exoatmospheric 
trajectories.  If  we  relax  these  assumptions,  then  the  simplicity 
of  the  ensuing  algorithm  will  be  lost. 

We  emphasize  that  Eq.  (3.1)  is  a convenient  and  suffi- 
cient way  of  representing  the  trajectory  estimation  problem  being 
considered.  When  the  trajectory  is  described  by  a differential 
equation  of  motion,  i.e., 

x = g (x)  ; x(tQ)  (3.1a) 

then  one  can  always  obtain  an  equivalent  discrete  system  by  using 
numerical  integration  to  evaluate 


£ (tn+l}  * 2(V  + / 2<£>dt 


/ 


n+1 


n 


= f (x  ) 
-n 


(3.1b) 


The  least  square  algorithm  to  be  described  works  with  a 
batch  of  measurements.  Let  $ , $j.'***^N  denote  a batch  of  N 


*The  ensuing  algorithm  is  applicable  to  systems  with  unknown 

constant  parameter  vectors. 


measurements . 

A 

-n'  n=1,  ***' 


One  would  like  to  obtain  an  estimated  state  sequence 
N so  that 
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i 

n=l 


r\j  T1  _ “I  'V, 

(y  -y  ) AR  (y  -y  ) 

'*-n  in'  n xn 


(3.3) 


is  minimized  subject  to  constraint  Eqs.  (3.1)  and  (3.2).  Since 
there  is  no  process  noise  assumed  in  (3.1),  one  is  only  required 
to  estimate  the  initial  state  (corresponding  to  n=l)  x^.  Further- 
more, if  an  inverse  function  of  f(),  i.e.. 


-n  _ f"1(2n+l) 


(3.4) 


can  be  found,  the  optimal  estimate  of  the  entire  trajectory  can 
be  obtained  if  one  minimizes  (3.3)  with  respect  to  any  state 
vector  along  the  trajectory.  In  our  trajectory  estimation  applica 
tion  the  f ( ) function  merely  corresponds  to  integration  back- 
wards in  time.  We  can  therefore  minimize  (3.3)  with  respect  to 
any  state  along  the  trajectory.  For  the  purpose  of  convenience, 
we  minimize  (3.3)  with  respect  to  the  initial  state  x. . 

Before  minimizing  (3.3)  can  be  made  tractable,  we 
introduce  approximations  to  the  system  and  measurement  equations. 

A 

Let  x“  denote  an  initial  guess  (estimate)  of  the  true  state  xft, 
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we  approximate  the  measurement  equation  by  using  a first  order 
Taylor  series  expansion  about  x°, 


Ln 


“ !!(Sn> 


h(x°)  + 


Hn  (Jn-Sn» 


(3.5) 


where  H°  is  the  Jacobian  matrix  of  h(x  ) evaluated  at  x°.  Using 
n — — n — n 

the  system  Eq. , (3.1)  and  iterating  n times  yields 


x * f_(x.)  (3.6a) 

-n  — n —l 

Note  that  if  (3.1)  is  a linear  system  then  f ( ) is  the  product 
of  n transition  matrices  and  if  (3.1a)  is  used  then 


g(x)dt 


(3.6b) 


Let  x°  denote  the  initial  guess  (estimate)  of  x^,  then  it  is 

related  to  the  x°  used  in  (3.5)  by 
-n 


(3.7) 


Approximating  the  system  equation  by  using  a first  order  Taylor 
series  expansion  about  x°  yields 


(3.8) 


*n  = fn(5i)  « fn(2?>  + Fn{* 1 “5?) 

where  F°  is  the  Jacobian  matrix  of  f ( ) evaluated  at  x? . 
n ~n 

We  now  minimize  (3.3).  Substituting  (3.5),  (3.7),  and 

(3.8)  in  (3.3)  yields 
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J « 


r <*n  - 


n=l 


Win'  + HnFn(^5?)])T  V* 


(3.9) 


<*n  " 


wi°n)  + 


Taking  the  derivative  of  J with  respect  to  x1  and  solving  for  x^ 
yields 
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(3.10) 


This 


Notice  that  we  have  used  the  notation  to  replace  x^. 

/■V 

algorithm,  if  it  converges,  produces  an  estimate  ^ which  is  better 

A - A 

than  the  initial  guess  x°.  If  we  use  in  the  place  of  the  ini- 
tial guess  and  go  through  the  calculation  again,  we  will  receive 
a further- improved  estimate  (provided  that  the  algorithm  converges) 
Thus  we  have  obtained  an  iterative  algorithm, 


'k+1 
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(3.11) 


~k+l 

and  the  covariance  of  is 


''k+l 

cov  (x*  x)  = 


f*  h*  r/hV 

n n n n n 


-1 


(3.12) 


The  iteration  is  terminated  when  the  difference  of  two  successive 
performance  indices  (Eq,.  (3.3))  is  below  a certain  threshold. 

We  make  the  following  remarks: 

1.  This  iterative  algorithm  processes  a batch  (N  points) 
of  data.  It  is  iterating  over  the  same  batch  of  data  in 
attempting  to  minimize  the  weighted  least  square  error.  IJ[ 
it  converges  and  is  terminated  with  a finite  number  of 
iterations,  it  produces  near  optimum  estimates.  This 
algorithm  is  fundamentally  different  from  the  commonly  known 
recursive  algorithms  such  as  the  extended  Kalman  filter  (EKF) . 


2.  The  convergence  property  of  this  algorithm  is 
determined  by  a)  the  properties  of  f ( ) and  h(  ) 

and  b)  the  initial  guess  x£*  Deriving  an  analytical 
convergence  criterion  for  the  above  algorithm  is  a 
rather  complicated  problem.  In  our  angle-only  tracking 
application,  numerical  experiences  indicate  that  if  the 
measurement  noise  is  sufficiently  low  then  a properly 
computed  always  converges  to  the  optimal  estimate 

3.  The  covariance  equation  Eq.  (3.12)  has  the 
identical  functional  form  as  the  Cramer-Rao  lower  bound 
on  the  covariance  of  trajectory  estimation  (Refs.  [1] 
and  [2]).  The  difference  is  that  the  Cramer-Rao  bound 
is  evaluated  using  the  true  states  while  Eq.  (3.12) 

is  evaluated  using  the  estimated  states.  This  implies 
that  if  the  least  square  estimate  converges  asympotically 
(when  the  number  of  measurements  is  large)  then  its 
covariance  approaches  the  Cramer-Rao  bound. 

4.  The  covariance  equation  is  apparently  related  to 
the  observability  condition  for  nonlinear  discrete 
systems.  We  note  that  this  subject  is  an  area  still  not 
very  well  understood.  Notice  that  the  matrix  to  be 
inverted  in  (3.12)  may  be  singular.  If  it  is 

singular  for  all  N,  then  this  algorithm  fails  and  the 
system  is  not  observable  in  the  weighted  Euclidean  norm 
sense. 


5.  Once  the  estimate  at  the  initial  time  is  found,  one 
may  calculate  state  estimates  at  any  arbitrary  time  by 
simply  applying  Eq.  (3.1).  The  associated  covariance 
can  be  approximated  by  using 


A 


cov  (xn) 


a np 

F_  cov  (x. ) F * 
n -I  n 


(3.13) 


The  above  procedure  is  valid  because  there  is  no  process 
noise  associated  with  the  system  dynamics.  If  one  attempts 
to  generalize  the  above  algorithm  by  introducing  process 
noise,  a much  more  complicated  optimization  problem  results. 
Fortunately,  for  the  exo-atmospheric  trajectory  estimation 
problem,  the  process  noise  is  negligible. 
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6.  One  may  use  the  above  procedure  to  compute  an 
initial  estimate  for  a general  recursive  tracking 
filter  such  as  the  Extended  Kalman  filter.  In  the 
angle-only  measurement  trajectory  estimation  problem, 
track  initiation  is  often  difficult.  The  above 
procedure  seems  to  provide  a reasonable  approach 

for  this  application. 

7.  Due  to  its  simplicity,  we  suggest  the 
following  procedure  for  tracking  application. 

7.1  Use  the  smallest  N,  so  that  the  matrix 

in  the  bracket  of  (3.12)  is  nonsingular,  to  compute 
an  estimate  at  initial  time.  The  estimate  at  any 
arbitrary  time  can  be  obtained  by  applying  the 
trajectory  equation. 

7.2  When  the  (N+l)st  measurement  is  available, 
use  the  estimate  obtained  in  (6.1)  above  as  an  initial 
guess  then  apply  the  algorithm.  Because  this  initial 
guess  is  the  optimum  estimate  for  N measurements,  it 
converges  to  the  optimal  estimate  for  M+l  measure- 
ments very  quickly. 

We  will  demonstrate  an  algorithm  for  computing  the 
initial  guess  used  in  (6.1)  for  the  angle-only  measurement  case 
in  the  next  subsection. 


3.2.  Initial  Guess  Calculation 

In  the  above  section,  we  have  illustrated  an  iterative 
least  square  algorithm  which  can  be  used  to  estimate  states  of 
ballistic  trajectories  with  angle  only  measurements.  This  algorithm, 
if  it  converges,  gives  near  optimum  estimates.  Its  convergence 
is,  however,  hinged  on  properly  choosing  an  initial  guess.  In  this 
section,  we  suggest  a procedure  for  computing  an  initial  guess 
using  a batch  of  data. 
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This  procedure  is  illustrated  in  Fig.  3.1.  A batch  of 
angle  measurements,  ( X , &n) , n»l,  N,  is  first  smoothed  by  a 

second  order  polynomial  to  obtain  their  derivatives.  The  poly- 
nomial fit  procedure  can  be  found  in  many  standard  textbooks.  A 
brief  but  fairly  general  analysis  was  presented  in  an  earlier 
report  [1] . We  simply  state  the  applicable  results  below.  Let 

0n,  n=l,  ...,  N denote  N angle  measurements  (which  can  be  either 

^ > * • ft 

A's  or  E's)f  then  the  0,  0,  and  9 corresponding  to  the  center  of 
the  data  interval  can  be  obtained  by 
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(3.14a) 
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(3.14b) 
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(3.14c) 


n=l 


where  T is  time  between  measurements. 
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Fig.  3.1.  Initial  guess  calculation  procedure 
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With  the  smoothed  angles,  one  may  apply  eqs.  (2.5b)  and 
(2.5c)  to  compute  R and  f*.  These  two  equations  are  restated  below. 


= -2  | A cos  E + A E tan  E 


(3.15a) 
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(3.15b) 


Notice  that  the  above  are  two  simultaneous  nonlinear  equations  with 
two  unknowns,  R and  R.*  They  may  be  solved  using  iterative  algorithms 
such  as  Picard's  method  or  Newton's  method  [4].  The  details  are 
omitted  here. 

There  is  another  method  useful  for  solving  R and  R.  This 
second  method  seems  to  have  better  convergence  property  and  it  is 
the  algorithm  used  in  the  simulation  study.  This  is  to  use  the 
fact  that  the  total  energy  of  a ballistic  object  is  unchanged 
throughout  its  exoatmospheric  flight.  Let  E^  denote  the  normalized 
(by  mass)  average  total  energy  of  a intercontenental  ballistic 
missile,  it  is  equal  to  the  sum  of  its  kinetic  energy  (E^)  and 
potential  energy  .(E  ) . 

r 


®t  * Ek  + EP 


VRe 

+ g_  R (~±~) 


k V 2 
I T 


*T 


(3.16) 


*If  the  term  involving  g is  absent,  the  equations  obtain  only 
the  combination  R/R  and°cannot  be  solved  for  R and  R separately. 
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where  VT  is  the  target  velocity,  gQ  the  gravitational  constant, 

Re  t^ie  radius  of  the  earth  and  RT  the  distance  between  the  target 
and  the  center  of  the  earth.  Let 


The  target  velocity  vector  in  a 
earth  centered  Cartesian  coordinate 


*s 


The  sensor  velocity  vector  in  a 
earth  centered  Cartesian  coordinate 


then 


xs  + aR 

ys  + bR 

+ CR 
s 


(3.17) 


where  a = ^ cos  E sin  A - E sin  A sin  E + A cos  E cos  A 

b = ^ cos  E cos  A - E cos  A sin  E - A sin  A cos  E 
c ~ ~ sin  E + E cos  E (3.18) 


Substituting  (3.1®  and  (3.17)  in  (3.16)  and  using  average  total 
missile  energy,  one  may  solve  for  R and  R together  with  Eq.  (3.15a). 
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I 

i 


With  R,  R,  A,  A,  E,  E,  one  may  calculate  the  state  vector 
in  a sensor  centered  Cartesian  coordinate  using  the  following 
equations 


3,3.  Incorporating  A Priori  Knowledge 


In  the  ballistic  trajectory  estimation  problem,  several 
trajectory  variables  such  as  re-entry  velocity,  re-entry  angle, 
etc.,  are  known  to  within  a certain  range  of  values.  Incorporating 
this  a priori  information  with  measurements  to  obtain  a "combined" 
estimate  constitutes  a constrained  estimation  problem.  We  will 
discuss  the  constrained  estimation  problem  in  Sec.  3.3.1.  Previous 
analytical  studies  [1,2]  indicated  that  the  re-entry  velocity 
constraint  is  the  most  significant  a prior  knowledge  and  it 
precludes  all  other  constraints.  For  this  reason  an  algorithm  for 
explicitly  incorporating  the  velocity  constraint  is  discussed  in 
Sec.  3.3.2. 


3.3.1.  Constrained  Estimation 

Let  denote  a known  constraint  set  which  the  state 
vector  x must  satisfy.  The  constrained  estimate  of  x based  upon 

'Ki  'Xj  , , 

both  U and  measurements  Yy  ' . . . , is  the  x satisfying 


min 

xe  fl, 


n=l 


T 


(*n-*n> 


(3.21) 


subject  to  system  and  measurement  Eqs.  (3.1)  and  (3.2).  Notice 
that  the  summation  in  the  bracket  is  identical  to  Eg.  (3.3). 
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The  above  minimization  can  be  decomposed  into  two  steps: 
1)  obtain  the  unconstrained  estimate  and  covariance  and  2)  modify 
the  unconstrained  estimate  using  the  constraint  set.  This  decom- 
position procedure  is  optimal  only  for  linear  systems.  We  never- 
theless adopt  it  here  because  the  optimal  solution  for  nonlinear 
systems  is  very  difficult  to  obtain. 

Let  x and  P denote  the  state  estimate  and  covariance 
obtained  by  the  algorithm  described  in  Sec.  3.1,  the  constrained 
estimate  x is  the  solution  of 


r % a t -i  ^ i 

min  l(x—x)  P (x-x)  (3.22) 

ft  L"  “ " 


The  above  equation  indicates  that  the  optimal  estimate  is  a state 
vector  in  ft  which  has  the  shortest  weighted  distance  to  the  un- 
constrained  estimate  x.  if  x is  in  ft,  then  x = x.  If  x is 

A 

outside  of  ft.,  then  x is  on  the  boundary  of  ft  . We  illustrate  a 
two-dimensional  constrained  estimation  in  Fig.  3.2.  Notice  that 

A i\, 

the  optimal  estimate  x may  not  be  the  closest  to  x measured  by 
using  the  conventional  distance.  This  is  because  that  the  distance 
implied  in  (3.22)  is  weighted  by  the  covariance  of  x. 

If  3t  is  an  unbiased  estimate  of  x and  is  Gaussian,  then 

A 

x obtained  in  using  (3.22)  is  the  maximum  likelihood  estimate  of 
x based  upon  both  measurement  and  constraint  set  ft.  If  a 


x> 


x2 


4 Unconstrained  Estimate 


TN  79-1(3.2) 


Fig.  3.2.  Constrained  estimation. 
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distribution  of  x in  ft  is  known,  one  may  also  use  the  minimum 

A 

mean  square  error  estimator  for  computing  x.  Since  the  minimum 
mean  square  error  estimation  requires  a distribution  of  x in  ft , 
and  it  is  also  more  difficult  to  compute,  we  will  not  discuss  it 
here.  Discussions  on  the  constrained  estimation  problem  can  be 
found  in  Ref.  [6]  . 

In  the  trajectory  estimation  application,  a constraint 
set  can  be  specified  by  bounds  on  velocity  (v) , heading  angle  (0) , 
and  re-entry  angle  (y)*.  We  denote  the  constraint  set  by 

ft  = jx;  vlv2'^l—  ^ — ^2'®1~®—  ^2}  (3.23) 

where  these  quantitites  are  related  to  the  coordinates  in  a 
east  (x)  - north  (y)  - up  (z)  oriented  Cartesian  coordinate  by 

» - <;2  + i2  ♦ ;2>1/2 

^ 1 X • 

y = tan  (y  = 0 when  heading  north) 

9 = sin'1  — (3.24) 

v ' 

% 

As  we  discussed  before,  when  x is  outside  of  ft,  then  the 
optimal  solution  lies  on  the  boundary  of  ft.  Using  (3.23),  it  is 


*One  may  also  include  energy  bounds  ft.  This  does  not  change 
the  generality  of  the  ensuing  discussions. 


evident  that  the  segment  of  the  boundary  which  contains  x is  a 
subset  of  variables  in  (3.23)  achieving  equality.  Such  a subset 
can  be  found  by  searching  through  all  possible  combinations 
of  variables  achieving  equalities.  This  procedure  is  tedious 
but  straightforward.  Let  f_  (x)  = c denote  such  a subset,  then  the 
minimization  problem  described  in  (3.22)  becomes 


min  [ J 


(x-x)1 


— 1 f\j  * 

P A(x-x) 


+ X (f  (x)  -e)  ] 


(3.25) 


where  is  the  vector  of  Lagrange  multipliers . The  necessary 
conditions  for  minimizing  J are 


-2P-1  (x-x)  + 


ax 


X = o 


(3.26) 


f(x)  = c 


(3.27) 


An  algorithm  solving  (3.26)  and  (3.27)  by  including  the 
constraint  set  described  in  (3.23)  can  be  quite  involved.  Previous 
analytical  studies  [1] , [2]  indicate  that  constraints  on  the  angles 
are  very  loose  compared  with  the  velocity  constraints.  Later 


numerical  results  will  also  substantiate  this  fact.  For  this 
reason  we  derive  an  algorithm  by  only  considering  the  velocity 
constraint  in  the  next  subsection. 

3.3.2.  Incorporating  the  Velocity  Constraint 
The  velocity  constraint  can  be  rewritten  as 


n 


v 


2 T 
Xl  i 5 Sx  < 


(3.28) 


where  x is  the  state  vector  in  Cartesian  coordinates  with  com- 


ponents  (x,y, z,x,y,z) , S is 

a matrix 

defined 

Vx3 

• 

1 

o3x3l 

<7  ss 

i 

D — 

03x3 

u 

r 

i 

i 

t3x3 

***  M 

and  I is  an  identity  matrix.  Assuming  x is  not  contained  in 
&v  and 


T„ 

x Sx 


where  vq  may  be  equal  to  either  or  v2  , then  Eqs.  (3.26) 
(3.27)  become 


-p“1(^-x)  + X Sx  * 0 


a rp  a 

xSx 


A 

Solving  for  x in  (3.26a)  yields 


x 


+ XS) 


Substituting  (3.29)  in  (3;27a)  yields 


3(TP“1(P“1+XS)“1S(P“1+XS)“1P"1>(  * Vq 


One  must  solve  for  X using  (3.30)  then  substitute  it  into  ( 
to  obtain  the  final  constrained  estimate  x.  We  use  Newton' 
method  to  solve  for  X in  (3.30).  Let 


and 


(3.26a) 


(3.27a) 


(3.29) 


(3.30) 


.29) 


27 


f (X) 


(3.31) 


* XTP~1(P_1+AS)~1S(P"1+XS)~1p“1x  - V3 


and 


f ' (X) 


df  (X) 

~ir~ 


(3.32) 


« -2xTp"1 (P-1+XS) _1S (P-1+AS) “1p“1x 


The  recursive  solution  for  X is 

f(Xn) 

Xn+1  " Xn  " r~a~j  (3*33) 

Results  of  applying  the  above  algorithm  to  the  angle- 
only  measurement  trajectory  estimation  problem  will  be  shown 
in  the  numerical  results  section. 

We  emphasize  that  although  the  constraint  equation  is  only 
explicitly  applied  to  the  velocity  components  of  the  state,  it  is 
expected  that  both  position  and  velocity  estimates  can  be  improved 
due  to  the  fact  that  they  are  correlated  through  the  covariance 
matrix  P.  This  point  can  be  made  clear  by  examining  (3.29).  Notice 

'V, 

that  all  components  of  x are  changed  by  the  constraint  if  X ft  0. 
Later  numerical  results  will  also  substantiate  this  fact. 

3.4.  Overall  Estimation  Algorithms 

We  now  combine  the  above  analyses  to  present  two  angle- 
only  tracking  algorithms.  One  of  which  is  strictly  using  the 
iterative  least  square  estimator  and  the  other  one  is  using  the 


28 


extended  Kalman  filter  with  the  initial  conditions  provided  by 
the  ILS  estimator.  These  two  algorithms  are  illustrated  in 
Figs.  3.3  and  3.4. 

Notice  that  both  algorithms  use  the  initial  guess 
calculation  method  presented  in  Section  3.2.  After  the 
initial  guess  has  been  obtained,  the  ILS  algorithm  proceeds  to 
recursively  apply  the  ILS  estimator.  Notice  that  the  initial  guess 
calculation  method  of  Section  3.2  does  not  suggest  ways  of  calculating 
the  initial  covariance  matrix.  In  order  to  initiate  the  EKF 
algorithm,  we  first  apply  ILS  to  obtain  both  initial  state  and 
covariance  then  proceed  to  recursively  apply  the  EKF  estimator. 

This  makes  the  ILS  and  EKF  initial  estimates  identical.  Later 
numerical  results  will  show  this  effect. 

Since  there  are  a number  of  iterative  processes  in  these 
algorithms,  these  following  rather  arbitrary  check  points  are 
established  to  detect  divergence: 

(1)  For  more  than  15  iterations,  AI=Ik+,-Ik  is  still 
larger  than  10”8  where 


~iT  v.  — 1 ~ l. 

x*  PK  xK 


10 

(2)  Estimated  altitude  is  larger  than  10  km. 

(3)  It  takes  more  than  1200  seconds  to  impact 

If  the  estimated  state  satisfies  any  one  of  the  above 
check  points,  one  additional  data  point  is  obtained  and  the 
initiation  procedure  is  started  over  again.  We  emphasize  that 
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Use  the  previous  | tn  79-1(3^3) 

x as  initial  guess 


Fig.  3.3.  Angie-only  track  algorithms:  The  iterative  least 
square  estimator. 


A batch  of  Ai's  and  E/s  (N) 


Pig.  3.4.  Angle-only  track  algorithms:  The  extended  Kalman 
Filter  Algorithm. 
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we  are  using  an  expanding  data  window  to  include  more  data  points 
in  the  initiation  process  as  oppose  to  using  a sliding  window. 

In  the  numerical  example  section,  we  will  use  an  example  to  illustrate 
the  convergence  frequency  as  a function  of  measurement  standard 
deviation  and  number  of  pulses  used  in  the  initiation  process. 

Notice  also  that  the  constrained  estimate  is  not  used 
in  the  feedback  loop.  That  is,  it  is  applied  only  at  the  output 
of  the  overall  algorithm. 


4. 


NUMERICAL  RESULTS 


In  this  section#  we  present  a numerical  example  to 
illustrate  our  results.  Both  the  free-falling  and  stationary 
sensor  platform  cases  are  considered.  The  sensor  and  target 
initial  state  vectors  in  an  arbitrarily  oriented  earth  centered 
Cartesian  coordinate  are 


x_  = [2000.,  3500.,  7000. ,-1.5, -3. , -2. ]T  in  km  and  km/s 

T 

x = [0. , 4200. ,6500. ,0. ,2. ,-l. ] in  km  and  km/s 

— s 

For  a free-falling  sensor  platform,  the  sensor  is  following  the 
same  differential  equation  of  motion  (eg.  (2.1))  with  the  above  x0  as 

— “S 

the  initial  state.  For  a stationary  sensor  platform.  The  sensor 
position  remains  constant  at  the  above  values.  This  could  be 
achieved  by  thrusting  the  sensor  platform  to  cancel  gravity  and 
velocity. 

We  assume  that  the  sensor  measurement  standard  deviation 
is  10  Urad.  Notice  from  Eq.  (3.12)  , the  estimation  covariance 
is  linearly  related  to  measurement  standard  deviations.  We  can 
therefore  scale  our  results  using  this  fact.  Higher  measurement 
standard  deviations  will  however,  result  in  difficulties  in 
covergence.  This  point  will  be  illustrated  later. 
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We  apply  velocity  constraints  at  the  filter  output 
to  improve  our  estimates.  Two  levels  of  velocity  bounds  are  used 
to  examine  the  sensitivity.  They  are  +.5  km/s  and  +.125  km/s  at 
the  true  velocity. 

The  simulation  program  is  run  in  a Monte  Carlo  fashion 
to  accumulate  the  error  statistics.  Root-mean- square  (RMS)  errors 
using  20  Monte  Carlo  runs  are  computed. 

The  RMS  position  errors  of ILS  and  EKF  estimates  for 
the  moving  sensor  platform  case  are  presented  in  Figs.  4.1  and 
4.2,  respectively.  Also  shown  is  the  unconstrained  Cramer-Rao 
bounds,  Ref.  [1],  [2],  It  is  evident  that  the  ILS  achieves  the 
Cramer-Rao  lower  bounds  while  the  EKF  does  not*.  It  is  also  in- 
teresting to  note  that  constrained  estimates  have  substantially 
smaller  errors  than  the  unconstrained  estimates  when  the  track 
time  is  short.  When  the  track  time  is  long,  however,  i.e.,  where 
there  are  sufficient  data  to  provide  velocity  estimates  with 
errors  better  than  the  bound,  the  advantage  of  constrained 
estimation  is  lost. 

The  RMS  position  errors  of  ILS  and  EKF  estimates  for 
the  stationary  sensor  platform  case  are  presented  in  Figs.  4.3  and 
4.4  respectively.  Notice  that  both  ILS  and  EKF  achieve  the  Cramer- 
Rao  bound.  The  estimation  error  of  a stationary  sensor  is  much 

*The  fact  that  both  EKF  and  ILS  are  below  the  C.R.  Bound  for  short 
times  is  artificial  and  is  due  to  assumptions  equivalent  to  a 
velocity  bound  for  initialization. 


RMS  P 


Time  - Second 


Fig.  4.1.  Position  estimation  error;  The  ILS  estimator  with 
a free-falling  sensor. 


RMS  Position  Error 


nconstrai 


u 


RMS  Position  Error 


Time  - Second 

g.  4.4.  Position  estimation  error:  The  EKF  algorithm  with 
stationary  sensor  (notice  the  vertical  scale  change  with 
gs.  4.1  and  4.2). 
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smaller  than  that  of  a free-falling  sensor  (notice  the  change  of 
vertical  scale) . A stationary  sensor  platform  is  apparently 
"enhancing"  the  system  "observability"  and  this  pushes  the  EKF 
much  closer  to  the  optimum  (in  the  Cramer-Rao  bound  sense) 
performance.  Furthermore,  the  velocity  constraints  seem  to  make 
very  little  improvement  over  the  unconstrained  estimates. 

Figs.  4.5  and  4.6  present  track  initiation  success 
frequencies  for  free-falling  and  stationary  platforms,  respectively. 
The  success  frequency  for  the  stationary  sensor  platform  case  is 
much  higher  than  that  for  the  free-falling  sensor  platform  case 
under  the  same  conditions.  Notice  that  the  success  frequency 
drops  quickly  as  oQ  increases.  For  a given  oQ , the  success 
frequency  increases  for  increasing  number  of  pulses  used  in  the 
initiation  process.  The  expanding  window  algorithm  suggested  in 
Section  3.4  therefore  insures  a near  100%  success  frequency  for 
a given  aQ . 


initiation  Success  Frequency 
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40  * Used  in  the 
3Q  _ Initiation  ■ 4 


20  30  40  50 


200  300  400  500 
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Fig.  4.5.  Track  initiation  success  frequency:  Free-falling 
sensor  platform. 
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5.  CONCLUSIONS 


We  have  derived  an  iterative  least  square  (ILS)  esti- 
mation  algorithm  and  applied  it  to  the  problem  of  ballistic  tra- 
jectory estimation  with  angle-only  measurements.  We  have  also 
suggested  an  initiation  scheme  which  is  applicable  to  initiating 
either  the  ILS  or  the  extended  Kalman  filter  (EKF)  algorithm. 
Methods  for  constrained  estimation  are  presented  to  incorporate 
a priori  trajectory  constraints  with  state  estimates. 

There  are  still  basic  open  issues  in  this 
area.  For  example/  theories  of  the  conditions  for  which 
the  ILS  will  converge,  the  relationships  among  observability  of 
nonlinear  systems,  the  Cramer-Rao  bound  , and  the  iterative  least 
square  algorithm,  among  others,  have  not  bedh  explored. 

It  is  found  that  a stationary  sensor  platform  achieves 
substantially  smaller  estimation  errors  than  the  free-falling 
sensor.  This  suggests  that  there  may  exist  an  optimum  control 
strategy  for  the  sensor  platform  such  that  the  estimation  error 
is  minimized.  This  is  also  an  open  area  for  further  research. 

In  addition  to  the  above  discussions,  we  draw  the  fol- 
lowing conclusions. 

(1)  For  the  free-falling  sensor  platform  case, 
the  ILS  estimator  achieves  the  Cramer-Rao  lower 
bound  on  the  covariance  of  trajectory  estimates  while 
the  EKF  does  not. 

(2)  For  the  stationary  sensor  platform  case, 
both  ILS  and  EKF  achieve  the  Cramer-Rao  lower  bound 
and  the  estimation  error  is  much  smaller  than  that 
for  the  free-falling  sensor  platform  case. 


(3)  For  sufficiently  small  measurement  standard 
deviations,  the  suggested  initiation  procedure  seems  to 
have  fine  convergence  properties. 

(4)  An  extremely  good  velocity  bound  (i.e.,  the  +.125  km/s 
case)  only  gives  marginal  improvement  over  unconstrained  estimates 
after  tracking  for  150  seconds,  for  the  free-falling  sensor 
platform  case.  For  the  stationary  platform  case,  the  velocity 
constraints  make  very  little  effect  over  the  unconstrained 
estimates. 
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